import time
try:
    import thread as _thread
except:
    import _thread


import Adafruit_PCA9685

FREE_ANGLE=181
SERVO_NUM = 64
# ADRESS_TABLE = [0x40,0x41,0x42,0x44,0x00,0x00,0x00,0x00]
ADRESS_TABLE = [0x42,0x47,0x41,0x40,0x44,0x00,0x00,0x00]

SPEED_CTL_ANGLE_INTERVAL = 0.8
ANGLE_TO_TIME = 1000
SPEED_COEFIENT = 1.8

# SPEED_CTL_T_INTERVAL = 2500
# FIRST_ANGLE = 3
# FIRST_ANGLE_THRES = 10


class servo_c():
    def __init__(self, num = SERVO_NUM, address = ADRESS_TABLE):
        self.servo_num = SERVO_NUM
        self.driver_address = address

        self.current_angles = [0] * self.servo_num 
        self.angles_to = [FREE_ANGLE] * self.servo_num 
        self.servo_speeds = [0] * self.servo_num 
        self.servo_speeds_t_record = [0] * self.servo_num 
        self.servo_speeds_interval_record = [0] * self.servo_num 

        self.pwm = [None] * (self.servo_num // 16)
        try:
            for i in range(len(self.pwm)):
                self.pwm[i] = Adafruit_PCA9685.PCA9685(self.driver_address[i], busnum=1)
                self.pwm[i].set_pwm_freq(50)
        except:
            print("servo driver error")
            
    def start_update_task(self):
        _thread.start_new_thread(self.update_task, ())

    def _run_to(self, idx, angle):
        driver_id = idx // 16
        servo_id = idx % 16
        # print("play servo", idx, driver_id, servo_id, angle)
        date = 4096 * ((angle * 11) + 500) / 20000
        try:
            self.pwm[driver_id].set_pwm(servo_id, 0, int(date))
        except:
            print("servo run to error")
        self.current_angles[idx] = angle

####################################################
    def free(self, idx):
        driver_id = idx // 16
        servo_id = idx % 16
        try:
            self.pwm[driver_id].set_pwm(servo_id, 0, 0)
            self.current_angles[idx] = FREE_ANGLE
        except:
            pass

    def set_angle(self, idx, angle, speed = 0, update = False):
        if idx >=  self.servo_num:
            print("invalid servo id")
            return
        
        self.angles_to[idx] = angle
        self.servo_speeds[idx] = speed
        self.servo_speeds_interval_record[idx] = 0

        if update:
            self.update()
    def _cal_delta_angle(self, to, current, speed):
        diff = to - current
        if speed == 0:
            return diff, abs(diff) * ANGLE_TO_TIME
        # elif abs(diff) > FIRST_ANGLE_THRES:
        #     return (FIRST_ANGLE if diff > 0 else -FIRST_ANGLE), FIRST_ANGLE * ANGLE_TO_TIME
        # else:
        #     return (SPEED_CTL_ANGLE_INTERVAL if diff > 0 else (-SPEED_CTL_ANGLE_INTERVAL)), \
        #     SPEED_CTL_ANGLE_INTERVAL * ANGLE_TO_TIME + speed * SPEED_CTL_T_INTERVAL
        else:
            angle = SPEED_CTL_ANGLE_INTERVAL * ((100 - speed) / 100) * SPEED_COEFIENT
            angle = angle if diff >= 0 else (-angle)
            inter = abs(angle * ANGLE_TO_TIME)
            return angle , inter

    def update(self):
        for i in range(self.servo_num):
            if self.angles_to[i] == FREE_ANGLE and (self.angles_to[i] != self.current_angles[i]):
                self.free(i)
                continue

            if abs(self.angles_to[i] - self.current_angles[i]) > SPEED_CTL_ANGLE_INTERVAL:
                if (time.time() * 1000000 - self.servo_speeds_t_record[i]) >= self.servo_speeds_interval_record[i]:
                    delta, run_t = self._cal_delta_angle(self.angles_to[i], self.current_angles[i], self.servo_speeds[i])
                    self._run_to(i, self.current_angles[i] + delta)
                    self.servo_speeds_t_record[i] = time.time() * 1000000
                    self.servo_speeds_interval_record[i] = run_t
            else:
                self.servo_speeds_interval_record[i] = 0

    def update_task(self):
        while 1:
            self.update()
            time.sleep(0.001)
            
####################################################
    def test(self):

        while 1:
            self.set_angle(0, 60, 0)
            time.sleep(1)
            self.set_angle(0, 110, 10)
            time.sleep(1)
            self.set_angle(0, 60, 0)
            time.sleep(1)
            self.set_angle(0, 110, 0)
            time.sleep(1)


servoCtl = servo_c()
